import matplotlib.pyplot as plt
from matplotlib.patches import Polygon, Rectangle, Circle
import numpy as np
import math
from grid_map import GridMap
import sys
import time

import ecal.core.core as ecal_core
import proto_messages.control_pb2 as control_pb2

sys.path.append("..")
from imu_pub.imu_proto_messages import imu_proto_pb2 as imu_proto_pb2
from ecal.core.publisher import ProtoPublisher
from ecal.core.subscriber import ProtoSubscriber

fig, axes = plt.subplots(figsize=(10, 20))

def obstacle_check(pose, gridmap, params):
    gmap = gridmap


    r = int(100 * params.sensor_range_m)
    back = [pose[0] - r * np.cos(pose[2]), pose[1] - r * np.sin(pose[2])]
    front = [pose[0] + r * np.cos(pose[2]), pose[1] + r * np.sin(pose[2])]
    left = [pose[0] + r * np.cos(pose[2] + np.pi / 2), pose[1] + r * np.sin(pose[2] + np.pi / 2)]
    right = [pose[0] - r * np.cos(pose[2] + np.pi / 2), pose[1] - r * np.sin(pose[2] + np.pi / 2)]

    pi = np.array(pose[:2], dtype=int)
    backi = np.array(back, dtype=int)
    fronti = np.array(front, dtype=int)
    lefti = np.array(left, dtype=int)
    righti = np.array(right, dtype=int)

    obstacle = {
        'front': 0,
        'back': 0,
        'right': 0,
        'left': 0,
    }

    for i in np.arange(min(pi[0], fronti[0]), max(pi[0], fronti[0]) + 1):
        for j in np.arange(min(pi[1], fronti[1]), max(pi[1], fronti[1]) + 1):
            m = min(j, gmap.shape[0] - 1)
            n = min(i, gmap.shape[1] - 1)
            if gmap[m, n]:
                # print('FRONT collision')
                obstacle['front'] = 1

    for i in np.arange(min(pi[0], backi[0]), max(pi[0], backi[0]) + 1):
        for j in np.arange(min(pi[1], backi[1]), max(pi[1], backi[1]) + 1):
            m = min(j, gmap.shape[0] - 1);
            n = min(i, gmap.shape[1] - 1)
            if gmap[m, n]:
                # print('BACK collision')
                obstacle['back'] = 1

    for i in np.arange(min(pi[0], lefti[0]), max(pi[0], lefti[0]) + 1):
        for j in np.arange(min(pi[1], lefti[1]), max(pi[1], lefti[1]) + 1):
            m = min(j, gmap.shape[0] - 1);
            n = min(i, gmap.shape[1] - 1)
            if gmap[m, n]:
                # print('LEFT collision')
                obstacle['left'] = 1

    for i in np.arange(min(pi[0], righti[0]), max(pi[0], righti[0]) + 1):
        for j in np.arange(min(pi[1], righti[1]), max(pi[1], righti[1]) + 1):
            m = min(j, gmap.shape[0] - 1);
            n = min(i, gmap.shape[1] - 1)
            if gmap[m, n]:
                # print('RIGHT collision')
                obstacle['right'] = 1

    return obstacle


def turn_right(pose, yaw):
    # goal_yaw=pose[2]-yaw
    pose[4] = 1 * math.sin(-yaw)  # omega(rad/s)
    print('turn_right', pose[4])
    return pose


def turn_left(pose, yaw ):
    # goal_yaw=pose[2]+yaw
    pose[4] = 1 * math.sin(yaw)  # omega(rad/s)

    return pose


def slow_down(state, params, dv=0.1):
    if state[3] > params.min_vel:
        state[3] -= dv
    return state




def motion(state, goal, params, initial_angle, Heading_rad, dt):
    # state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    dx = goal[0] - state[0]
    dy = goal[1] - state[1]
    goal_yaw = math.atan2(dy, dx)
    K_theta = 1.5
    car_angle = Heading_rad - initial_angle
    angle_diff = math.sin(goal_yaw - car_angle)
    print('angle_diff', angle_diff)
    # print('goal_yaw,car_angle,initial_angle,Heading_rad',goal_yaw,car_angle,initial_angle,Heading_rad)
    state[4] = K_theta * math.sin(goal_yaw - car_angle)  # omega(rad/s)
    state[2] = car_angle  # yaw(rad)

    dist_to_goal = np.linalg.norm(goal - state[:2])
    K_v = 0.1
    state[3] += K_v * dist_to_goal
    if state[3] >= params.max_vel: state[3] = params.max_vel
    if state[3] <= params.min_vel: state[3] = params.min_vel

    if np.abs(angle_diff) > 3.1415926 / 4:
        state[3] = state[3] * 0.0
    print('state', state)
    dv = dt * state[3]
    state[0] += dv * np.cos(state[2])  # x(m)
    state[1] += dv * np.sin(state[2])  # y(m)
    # print('dx,dy',dx,dy,'goal[0],goal[1]',goal[0],goal[1],'state[0],state[1]',state[0],state[1])
    return state

'''
障碍物防碰撞，暂时没有使用
'''
def collision_avoidance(state, gridmap, params):
    pose_grid = gridmap.meters2grid(state[:2])
    boundary = obstacle_check([pose_grid[0], pose_grid[1], state[2]], gridmap.gmap, params)
    print(boundary)
    # initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    if boundary['right'] or boundary['front']:
        # state = back_shift(state, 0.03) [ 2.02833509e+00  3.04950528e+00  1.53260330e+00  5.00000000e-01, -2.91890072e-04]
        state = slow_down(state, params)
        state = turn_left(state, np.radians(40))
    # state = forward_shift(state, 0.02)
    elif boundary['left']:
        # state = back_shift(state, 0.03)
        state = slow_down(state, params)
        state = turn_right(state, np.radians(40))
    # state = forward_shift(state, 0.02)
    return state


def define_flight_area(initial_pose):
    plt.grid()

    with open(
            "../coverage_path_planning/MapInformation/PolygonVertex_new.txt",
            "r") as file:
        lines = file.readlines()
    print('lines', lines)
    # 创建一个空列表来存储读取的数据
    PolygonVertex = []

    # 解析文件中的每一行数据
    for line in lines:
        # 按空格分割每行数据，并将其转换为整数
        x, y = map(int, line.strip().split())

        # 创建一个点对象并添加到列表中
        point = (x, y)
        PolygonVertex.append(point)
    flight_area_vertices = np.array(PolygonVertex) / 100

    return flight_area_vertices


class Params:
    def __init__(self):
        self.numiters = 1000
        self.animate = 0
        self.dt = 0.1
        self.goal_tol = 0.25
        self.max_vel = 0.4  # m/s
        self.min_vel = 0.2  # m/s
        self.max_omega_vel = 0.5  # rad/s
        self.min_omega_vel = 0.1  # rad/s
        self.sensor_range_m = 0.25  # m
        self.time_to_switch_goal = 5.0  # sec #inactive for now
        self.sweep_resolution = 0.4  # m
        self.start_pos = [1, 2.5]


Roll_rad, Pitch_rad, Heading_rad, RollSpeed, PitchSpeed, HeadingSpeed = None, None, None, None, None, None


def imu_callback(topic_name, imu_msg, time):
    '''print("Roll_rad: {},\nPitch_rad:{},\nHeading_rad:{},\nRollSpeed:{},\nPitchSpeed:{},\nHeadingSpeed:{}".format(imu_msg.Roll_rad,
                                                                           imu_msg.Pitch_rad,
                                                                           imu_msg.Heading_rad,
                                                                           imu_msg.RollSpeed,
                                                                           imu_msg.PitchSpeed,
                                                                           imu_msg.HeadingSpeed
                                                                           ))'''
    global Roll_rad, Pitch_rad, Heading_rad, RollSpeed, PitchSpeed, HeadingSpeed
    Roll_rad, Pitch_rad, Heading_rad, RollSpeed, PitchSpeed, HeadingSpeed = imu_msg.Roll_rad, imu_msg.Pitch_rad, imu_msg.Heading_rad, imu_msg.RollSpeed, imu_msg.PitchSpeed, imu_msg.HeadingSpeed


def main():
    obstacles = [ # 用来储存障碍物的信息，目前暂未使用
        np.array([[1.5, 3], [2.0, 3], [2.0, 3.5], [1.5, 3.5]])
    ]
    params = Params()
    # initial state = [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    state = np.array([params.start_pos[0], params.start_pos[1], 0.0, 0.0, 0.0]) # 储存车的速度位置等数据
    traj = state[:2]

    flight_area_vertices = define_flight_area(state[:2]) # 读取地图坐标点文件，返回相对位置浮点数坐标点np数组
    print('flight_area_vertices', flight_area_vertices)

    gridmap = GridMap(flight_area_vertices, state[:2]) # 把读取到的地图数据网格化，为了实现避障使用，目前暂未使用
    # gridmap.add_obstacles_to_grid_map(obstacles)

    CCP_txt = '../coverage_path_planning/result/coverage_path_coordinate.txt'
    with open(CCP_txt, 'r') as f:
        lines = f.readlines()
        data_array = []
        for i in lines[6:]:
            num = list(map(str, i.split(',')))  # 关于map（）函数另一篇文章有详细介绍，把字符串按照逗号分隔并转换成浮点数
            data_array.append([float(num[0]), float(num[1])])
        data_array = data_array[4:-2]
        data_array = np.array(data_array)
    goal_x, goal_y = data_array[:, 0], data_array[:, 1]

    goali = 0
    goal = [goal_x[goali], goal_y[goali]]
    t_prev_goal = time.time()
    # gridmap.draw_map(obstacles)
    iter = 0
    #########################################################
    ecal_core.initialize(sys.argv, "Python Protobuf Publisher")

    ecalcontrolMovepublisher = ProtoPublisher("ControlMessage", control_pb2.controlMoveOrder)
    sub = ProtoSubscriber("ImuMessage_topic", imu_proto_pb2.controlImu)

    # Set the Callback
    sub.set_callback(imu_callback)

    initial_angle = None
    last_time = None
    while True:
        global Roll_rad, Pitch_rad, Heading_rad, nRollSpeed, PitchSpeed, HeadingSpeed

        if Heading_rad == None:
            continue
        elif initial_angle == None:
            initial_angle = Heading_rad - 3.1415926 / 2 # 设置了一个默认初始角度是90°
            last_time = time.time()

        protobuf_message = control_pb2.controlMoveOrder()
        # protobuf_message.Header = "RealTimeMotion"
        now_time = time.time()
        dt = now_time - last_time
        state = motion(state, goal, params, initial_angle, Heading_rad,
                       dt)  # [ 2.02833509e+00  3.04950528e+00  8.34471604e-01  4.00000000e-01, -2.91890072e-04]

        state = collision_avoidance(state, gridmap, params) # 避障部分，暂时没有用到
        protobuf_message.lineSpeed = state[3]

        protobuf_message.angularSpeed = state[4]
        protobuf_message.pos_x = state[0]
        protobuf_message.pos_y = state[1]
        protobuf_message.car_dir = state[2]
        if protobuf_message.goal_pos == []:
            protobuf_message.goal_pos.append(goal[0])
            protobuf_message.goal_pos.append(goal[1])
        else:
            protobuf_message.goal_pos[0] = goal[0]
            protobuf_message.goal_pos[1] = goal[1]
        # print('set_linespeed，angularSpeed',protobuf_message.lineSpeed,protobuf_message.angularSpeed)

        # protobuf_message.lineSpeed = move_speed
        # protobuf_message.angularSpeed = out_angle
        # state[-1] = 0
        ecalcontrolMovepublisher.send(protobuf_message)
        last_time = now_time

        goal_dist = np.linalg.norm(goal - state[:2])
        # print('Distance to goal %.2f [m]:' %goal_dist)
        t_current = time.time()
        # if goal_dist < params.goal_tol or (t_current - t_prev_goal) > params.time_to_switch_goal: # goal is reached
        if goal_dist < params.goal_tol:  # goal is reached

            print('Switching to the next goal.')
            print('Time from the previous reached goal:', t_current - t_prev_goal)
            if goali < len(goal_x) - 1:
                goali += 1
            else:
                break
            t_prev_goal = time.time()
            goal = [goal_x[goali], goal_y[goali]]
        traj = np.vstack([traj, state[:2]])


        iter = iter + 1

    print('Mission is complete!')

    ecal_core.finalize()


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        pass
